/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package br.ufrn.engcomp.mpc.domain;

import br.ufrn.engcomp.mpc.util.MathCommonsUtils;
import org.apache.commons.math.linear.Array2DRowRealMatrix;
import org.apache.commons.math.linear.RealMatrix;

/**
 *
 * @author Victor
 */
public class CoupledTanksPlantWithDisturb extends CoupledTanksPlant {

    private RealMatrix extendedAMatrix;
    private RealMatrix extendedBMatrix;
    private RealMatrix extendedCMatrix;
    private RealMatrix fMatrix;
    private double disturb; // Distúrbio fictício
    private double setPoint; // n deve ficar aqui

    public CoupledTanksPlantWithDisturb() {
        // TESTE
        disturb = 1; // como calcular???

        extendedAMatrix = new Array2DRowRealMatrix(3, 3);
        extendedBMatrix = new Array2DRowRealMatrix(3, 1);
        extendedCMatrix = new Array2DRowRealMatrix(1, 3);
        fMatrix = new Array2DRowRealMatrix(3, 1);

        // Matriz coluna F = [0 1]' -> [0 1 1]'
        fMatrix.setEntry(0, 0, 0.0);
        fMatrix.setEntry(1, 0, 1.0);
        fMatrix.setEntry(2, 0, 1.0);
        
        // Novo A = [A F; 0 1]
        extendedAMatrix.setSubMatrix(aMatrix.getData(), 0, 0); // A
        extendedAMatrix.setColumnVector(2, MathCommonsUtils.convertMatrixToColumnVector(fMatrix)); // F
        extendedAMatrix.setEntry(2, 0, 0);
        extendedAMatrix.setEntry(2, 1, 1);

        // Novo B = [B 0]'
        extendedBMatrix.setEntry(0, 0, bMatrix.getEntry(0, 0));
        extendedBMatrix.setEntry(1, 0, bMatrix.getEntry(1, 0));
        extendedBMatrix.setEntry(2, 0, 0.0);

        // Novo C = [C 0]
        extendedCMatrix.setEntry(0, 0, cMatrix.getEntry(0, 0));
        extendedCMatrix.setEntry(0, 1, cMatrix.getEntry(0, 1));
        extendedCMatrix.setEntry(0, 2, 0.0);
    }

    public RealMatrix getFMatrix() {
        return fMatrix;
    }

    public void setFMatrix(RealMatrix fMatrix) {
        this.fMatrix = fMatrix;
    }

    public double getDisturb() {
        return disturb;
    }

    public void setDisturb(double disturb) {
        this.disturb = disturb;
    }

    public RealMatrix getExtendedAMatrix() {
        return extendedAMatrix;
    }

    public void setExtendedAMatrix(RealMatrix extendedAMatrix) {
        this.extendedAMatrix = extendedAMatrix;
    }

    public RealMatrix getExtendedBMatrix() {
        return extendedBMatrix;
    }

    public void setExtendedBMatrix(RealMatrix extendedBMatrix) {
        this.extendedBMatrix = extendedBMatrix;
    }

    public RealMatrix getExtendedCMatrix() {
        return extendedCMatrix;
    }

    public void setExtendedCMatrix(RealMatrix extendedCMatrix) {
        this.extendedCMatrix = extendedCMatrix;
    }

    public double getSetPoint() {
        return setPoint;
    }

    public void setSetPoint(double setPoint) {
        this.setPoint = setPoint;
    }

//    @Override
//    public double[] computeXModel(double[] lastX, double[] lastU) {
//        RealVector lastXVector = new ArrayRealVector(lastX);
//        RealVector lastUVector = new ArrayRealVector(lastU);
//        RealMatrix lastXVectorAsMatrix = MathCommonsUtils.convertColumnVectorToMatrix(lastXVector);
//        RealMatrix lastUVectorAsMatrix = MathCommonsUtils.convertColumnVectorToMatrix(lastUVector);
//
//        return MathCommonsUtils.convertMatrixToColumnVector(extendedAMatrix.multiply(lastXVectorAsMatrix).add(extendedBMatrix.multiply(lastUVectorAsMatrix))).getData();
//    }
//
//    // Calcula y(k) = Cx(k)
//    // é pra receber lastX ou X atual???
//    @Override
//    public double[] computeYModel(double[] lastX) {
//        RealVector lastXVector = new ArrayRealVector(lastX);
//        RealMatrix lastXVectorAsMatrix = MathCommonsUtils.convertColumnVectorToMatrix(lastXVector);
//        // Cx
//        return MathCommonsUtils.convertMatrixToColumnVector(extendedCMatrix.multiply(lastXVectorAsMatrix)).getData();
//    }
    
}
